﻿using System;
using System.Globalization;

namespace CoreRay
{
    //-----------------------------------------------------------------------------------------
    /// <summary>
    /// Class that represents a 4x4 matrix.
    /// </summary>
    //-----------------------------------------------------------------------------------------

    public class Matrix : IEquatable<Matrix>
	{
        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Initializes a new instance of the <see cref="Matrix"/> class.
        /// </summary>
        //-----------------------------------------------------------------------------------------

        public Matrix()
        {
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Initializes a new instance of the <see cref="Matrix"/> class.
        /// </summary>
        /// <param name="element00">The element at row 0 column 0.</param>
        /// <param name="element01">The element at row 0 column 1.</param>
        /// <param name="element02">The element at row 0 column 2.</param>
        /// <param name="element03">The element at row 0 column 3.</param>
        /// <param name="element00">The element at row 1 column 0.</param>
        /// <param name="element01">The element at row 1 column 1.</param>
        /// <param name="element02">The element at row 1 column 2.</param>
        /// <param name="element03">The element at row 1 column 3.</param>
        /// <param name="element00">The element at row 2 column 0.</param>
        /// <param name="element01">The element at row 2 column 1.</param>
        /// <param name="element02">The element at row 2 column 2.</param>
        /// <param name="element03">The element at row 2 column 3.</param>
        /// <param name="element00">The element at row 3 column 0.</param>
        /// <param name="element01">The element at row 3 column 1.</param>
        /// <param name="element02">The element at row 3 column 2.</param>
        /// <param name="element03">The element at row 3 column 3.</param>
        //-----------------------------------------------------------------------------------------

        public Matrix(float element00, float element01, float element02, float element03, float element10, float element11, float element12, float element13, float element20, float element21, float element22, float element23, float element30, float element31, float element32, float element33)
        {
            this.Element00 = element00;
            this.Element01 = element01;
            this.Element02 = element02;
            this.Element03 = element03;

            this.Element10 = element10;
            this.Element11 = element11;
            this.Element12 = element12;
            this.Element13 = element13;

            this.Element20 = element20;
            this.Element21 = element21;
            this.Element22 = element22;
            this.Element23 = element23;

            this.Element30 = element30;
            this.Element31 = element31;
            this.Element32 = element32;
            this.Element33 = element33;
        }

        #region Matrix Elements
        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 0 column 0.
        /// </summary>
        /// <value>The element at row 0 column 0.</value>
        //-----------------------------------------------------------------------------------------

        public float Element00
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 0 column 1.
        /// </summary>
        /// <value>The element at row 0 column 1.</value>
        //-----------------------------------------------------------------------------------------

        public float Element01
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 0 column 2.
        /// </summary>
        /// <value>The element at row 0 column 2.</value>
        //-----------------------------------------------------------------------------------------

        public float Element02
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 0 column 3.
        /// </summary>
        /// <value>The element at row 0 column 3.</value>
        //-----------------------------------------------------------------------------------------

        public float Element03
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 1 column 0.
        /// </summary>
        /// <value>The element at row 1 column 0.</value>
        //-----------------------------------------------------------------------------------------

        public float Element10
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 1 column 1.
        /// </summary>
        /// <value>The element at row 1 column 1.</value>
        //-----------------------------------------------------------------------------------------

        public float Element11
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 1 column 2.
        /// </summary>
        /// <value>The element at row 1 column 2.</value>
        //-----------------------------------------------------------------------------------------

        public float Element12
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 1 column 3.
        /// </summary>
        /// <value>The element at row 1 column 3.</value>
        //-----------------------------------------------------------------------------------------

        public float Element13
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 2 column 0.
        /// </summary>
        /// <value>The element at row 2 column 0.</value>
        //-----------------------------------------------------------------------------------------

        public float Element20
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 2 column 1.
        /// </summary>
        /// <value>The element at row 2 column 1.</value>
        //-----------------------------------------------------------------------------------------

        public float Element21
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 2 column 2.
        /// </summary>
        /// <value>The element at row 2 column 2.</value>
        //-----------------------------------------------------------------------------------------

        public float Element22
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 2 column 3.
        /// </summary>
        /// <value>The element at row 2 column 3.</value>
        //-----------------------------------------------------------------------------------------

        public float Element23
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 3 column 0.
        /// </summary>
        /// <value>The element at row 3 column 0.</value>
        //-----------------------------------------------------------------------------------------

        public float Element30
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 3 column 1.
        /// </summary>
        /// <value>The element at row 3 column 1.</value>
        //-----------------------------------------------------------------------------------------

        public float Element31
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 3 column 2.
        /// </summary>
        /// <value>The element at row 3 column 2.</value>
        //-----------------------------------------------------------------------------------------

        public float Element32
        {
            get;
            private set;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the element at row 3 column 3.
        /// </summary>
        /// <value>The element at row 3 column 3.</value>
        //-----------------------------------------------------------------------------------------

        public float Element33
        {
            get;
            private set;
        }
        #endregion

        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the identity matrix.
        /// </summary>
        /// <value>The identity matrix.</value>
        //-----------------------------------------------------------------------------------------

        public static Matrix Identity
		{
			get
			{
                Matrix matrix = new Matrix();

                matrix.Element00 = 1.0f;
                matrix.Element11 = 1.0f;
                matrix.Element22 = 1.0f;
                matrix.Element33 = 1.0f;

                return matrix;
			}
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the translation part of the matrix.
        /// </summary>
        /// <value>The translation part of the matrix.</value>
        //-----------------------------------------------------------------------------------------

        public Vector3 TranslationPart
		{
			get
			{
                return new Vector3(this.Element30, this.Element31, this.Element32);
			}
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Gets the determinant of the matrix.
        /// </summary>
        /// <value>The determinant of the matrix.</value>
        //-----------------------------------------------------------------------------------------

        public float Determinant
        {
            get
            {
                return this.Element00 * (this.Element11 * this.Element22 - this.Element12 * this.Element21) + this.Element01 * (this.Element12 * this.Element20 - this.Element10 * this.Element22) + this.Element02 * (this.Element10 * this.Element21 - this.Element11 * this.Element20);
            }
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a translation matrix.
        /// </summary>
        /// <param name="translationVector">The translation vector.</param>
        /// <returns>Translation matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix Translation(Vector3 translationVector)
		{
			//Fill the matrix
			Matrix result = Matrix.Identity;

            result.Element30 = translationVector.X;
            result.Element31 = translationVector.Y;
            result.Element32 = translationVector.Z;

			//Return the translation matrix
			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a rotation matrix around the X axis.
        /// </summary>
        /// <param name="angle">Angle for the rotation.</param>
        /// <returns>Rotation matrix.</returns>
        //-----------------------------------------------------------------------------------------

		public static Matrix RotationX(float angle)
		{
			//Fill the matrix
			Matrix result = Matrix.Identity;

			float cosAngle = (float) System.Math.Cos(angle);
			float sinAngle = (float) System.Math.Sin(angle);

            result.Element11 = cosAngle;
            result.Element12 = sinAngle;
            result.Element21 = -sinAngle;
            result.Element22 = cosAngle;

			//Return the rotation matrix
			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a rotation matrix around the Y axis.
        /// </summary>
        /// <param name="angle">Angle for the rotation.</param>
        /// <returns>Rotation matrix.</returns>
        //-----------------------------------------------------------------------------------------

		public static Matrix RotationY(float angle)
		{
			//Fill the matrix
			Matrix result = Matrix.Identity;

			float cosAngle = (float) System.Math.Cos(angle);
			float sinAngle = (float) System.Math.Sin(angle);

            result.Element00 = cosAngle;
            result.Element02 = -sinAngle;
            result.Element20 = sinAngle;
            result.Element22 = cosAngle;

			//Return the rotation matrix
			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a rotation matrix around the Z axis.
        /// </summary>
        /// <param name="angle">Angle for the rotation.</param>
        /// <returns>Rotation matrix.</returns>
        //-----------------------------------------------------------------------------------------

		public static Matrix RotationZ(float angle)
		{
			//Fill the matrix
			Matrix result = Matrix.Identity;

			float cosAngle = (float) System.Math.Cos(angle);
			float sinAngle = (float) System.Math.Sin(angle);

            result.Element00 = cosAngle;
            result.Element01 = sinAngle;
            result.Element10 = -sinAngle;
            result.Element11 = cosAngle;

			//Return the rotation matrix
			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs an uniform scale matrix.
        /// </summary>
        /// <param name="scale">The scale factor.</param>
        /// <returns>Scale matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix Scale(float scale)
        {
            //Fill the matrix
            Matrix result = Matrix.Identity;

            result.Element00 = scale;
            result.Element11 = scale;
            result.Element22 = scale;

            return result;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a scale matrix on the X axis.
        /// </summary>
        /// <param name="scale">The scale factor.</param>
        /// <returns>Scale matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix ScaleX(float scale)
        {
            //Fill the matrix
            Matrix result = Matrix.Identity;

            result.Element00 = scale;

            return result;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a scale matrix on the Y axis.
        /// </summary>
        /// <param name="scale">The scale factor.</param>
        /// <returns>Scale matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix ScaleY(float scale)
        {
            //Fill the matrix
            Matrix result = Matrix.Identity;

            result.Element11 = scale;

            return result;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a scale matrix on the Z axis.
        /// </summary>
        /// <param name="scale">The scale factor.</param>
        /// <returns>Scale matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix ScaleZ(float scale)
        {
            //Fill the matrix
            Matrix result = Matrix.Identity;

            result.Element22 = scale;

            return result;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a reflection matrix on the X axis.
        /// </summary>
        /// <returns>Reflection matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix ReflectX()
		{
			//Fill the matrix
			Matrix result = Matrix.Identity;

            result.Element00 = -1.0f;

			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a reflection matrix on the Y axis.
        /// </summary>
        /// <returns>Reflection matrix.</returns>
        //-----------------------------------------------------------------------------------------

		public static Matrix ReflectY()
		{
			//Fill the matrix
			Matrix result = Matrix.Identity;

            result.Element11 = -1.0f;

			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a reflection matrix on the Z axis.
        /// </summary>
        /// <returns>Reflection matrix.</returns>
        //-----------------------------------------------------------------------------------------

		public static Matrix ReflectZ()
		{
			//Fill the matrix
			Matrix result = Matrix.Identity;

            result.Element22 = -1.0f;

			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Constructs a lookat view matrix used to transform world space to view space.
        /// </summary>
        /// <param name="cameraPosition">The camera position.</param>
        /// <param name="cameraTarget">The camera target.</param>
        /// <param name="cameraUpVector">The camera up vector.</param>
        /// <returns>Look at matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix LookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)
		{
			//Calculate base axis
			Vector3 zAxis = (cameraTarget - cameraPosition).Normalize();
			Vector3 xAxis = Vector3.CrossProduct(cameraUpVector,zAxis).Normalize();
			Vector3 yAxis = Vector3.CrossProduct(zAxis,xAxis).Normalize();

			//Fill the matrix
			Matrix result = Matrix.Identity;

            result.Element00 = xAxis.X;
            result.Element01 = yAxis.X;
            result.Element02 = zAxis.X;

            result.Element10 = xAxis.Y;
            result.Element11 = yAxis.Y;
            result.Element12 = zAxis.Y;

            result.Element20 = xAxis.Z;
            result.Element21 = yAxis.Z;
            result.Element22 = zAxis.Z;

            result.Element30 = -(xAxis * cameraPosition);
            result.Element31 = -(yAxis * cameraPosition);
            result.Element32 = -(zAxis * cameraPosition);

			//Return the look at matrix
			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Construct a perspective projection matrix used to transform view space to projection 
        /// space.
        /// </summary>
        /// <param name="fieldOfViewY">The Y field of view in radian.</param>
        /// <param name="aspectRatio">The aspect ratio.</param>
        /// <param name="minPlaneZ">The Z min plane.</param>
        /// <param name="maxPlaneZ">The Z max plane.</param>
        /// <returns>Perspective projection matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix PerspectiveFov(float fieldOfViewY, float aspectRatio, float minPlaneZ, float maxPlaneZ)
		{
			float height = 1.0f / (float) System.Math.Tan(fieldOfViewY / 2.0f);

			Matrix result = Matrix.Identity;

            result.Element00 = height / aspectRatio;
            result.Element11 = height;
            result.Element22 = maxPlaneZ / (maxPlaneZ - minPlaneZ);
            result.Element32 = -minPlaneZ * maxPlaneZ / (maxPlaneZ - minPlaneZ);

            result.Element23 = 1.0f;
            result.Element33 = 0.0f;

			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Construct an orthogonal projection matrix used to transform view space to projection 
        /// space.
        /// </summary>
        /// <param name="width">The width.</param>
        /// <param name="height">The height.</param>
        /// <param name="minPlaneZ">The Z min plane.</param>
        /// <param name="maxPlaneZ">The Z max plane.</param>
        /// <returns>Orthogonal projection matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix Ortho(float width, float height, float minPlaneZ, float maxPlaneZ)
		{
			Matrix result = Matrix.Identity;

			if(width != 0)
			{
                result.Element00 = 2.0f / width;
			}

			if(height != 0)
			{
                result.Element11 = 2.0f / height;
			}

            result.Element22 = 1.0f / (maxPlaneZ - minPlaneZ);
            result.Element32 = minPlaneZ / (minPlaneZ - maxPlaneZ);

			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Construct an orthogonal projection matrix used to transform view space to projection 
        /// space.
        /// </summary>
        /// <param name="minPlaneX">The X min plane.</param>
        /// <param name="maxPlaneX">The X max plane.</param>
        /// <param name="minPlaneY">The Y min plane.</param>
        /// <param name="maxPlaneY">The Y max plane.</param>
        /// <param name="minPlaneZ">The Z min plane.</param>
        /// <param name="maxPlaneZ">The Z max plane.</param>
        /// <returns>Orthogonal projection matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix OrthoOffCenter(float minPlaneX, float maxPlaneX, float minPlaneY, float maxPlaneY, float minPlaneZ, float maxPlaneZ)
        {
            Matrix result = Matrix.Identity;

            result.Element00 = 2.0f / (maxPlaneX - minPlaneX);
            result.Element11 = -2.0f / (maxPlaneY - minPlaneY);
            result.Element22 = 1.0f / (maxPlaneZ - minPlaneZ);

            result.Element30 = (minPlaneX + maxPlaneX) / (minPlaneX - maxPlaneX);
            result.Element31 = -(minPlaneY + maxPlaneY) / (minPlaneY - maxPlaneY);
            result.Element32 = minPlaneZ / (minPlaneZ - maxPlaneZ);

            return result;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Computes the inverse of the matrix.
        /// </summary>
        /// <returns>Inverse of the matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public Matrix Inverse()
        {
            //Compute the determinant
            float determinant = this.Determinant;

            //Check to see if the inverse of this matrix is possible
            if(Math.Abs(determinant) < 0.000001f)
            {
                throw new InvalidOperationException("Could not inverse the current Matrix.");
            }

            float oneOverDeterminant = 1.0f / determinant;

            //Compute the 3x3 portion of the inverse, by dividing the adjoint by the determinant
            Matrix result = new Matrix();

            result.Element00 = (this.Element11 * this.Element22 - this.Element12 * this.Element21) * oneOverDeterminant;
            result.Element01 = (this.Element02 * this.Element21 - this.Element01 * this.Element22) * oneOverDeterminant;
            result.Element02 = (this.Element01 * this.Element12 - this.Element02 * this.Element11) * oneOverDeterminant;

            result.Element10 = (this.Element12 * this.Element20 - this.Element10 * this.Element22) * oneOverDeterminant;
            result.Element11 = (this.Element00 * this.Element22 - this.Element02 * this.Element20) * oneOverDeterminant;
            result.Element12 = (this.Element02 * this.Element10 - this.Element00 * this.Element12) * oneOverDeterminant;

            result.Element20 = (this.Element10 * this.Element21 - this.Element11 * this.Element20) * oneOverDeterminant;
            result.Element21 = (this.Element01 * this.Element20 - this.Element00 * this.Element21) * oneOverDeterminant;
            result.Element22 = (this.Element00 * this.Element11 - this.Element01 * this.Element10) * oneOverDeterminant;

            //Compute the translation portion of the inverse
            result.Element30 = -(this.Element30 * result.Element00 + this.Element31 * result.Element10 + this.Element32 * result.Element20);
            result.Element31 = -(this.Element30 * result.Element01 + this.Element31 * result.Element11 + this.Element32 * result.Element21);
            result.Element32 = -(this.Element30 * result.Element02 + this.Element31 * result.Element12 + this.Element32 * result.Element22);

            //HACK: Used to invert a projection Matrix
            if(this.Element33 == 0)
            {
                result.Element22 = 0.0f;
                result.Element33 = this.Element22 / this.Element23;
                result.Element32 = 1.0f / this.Element23;
            }

            else
            {
                result.Element33 = 1.0f;
            }

            return result;
        }

        public Matrix Transpose()
        {
            Matrix result = new Matrix();

            result.Element00 = this.Element00;
            result.Element01 = this.Element10;
            result.Element02 = this.Element20;
            result.Element03 = this.Element30;

            result.Element10 = this.Element01;
            result.Element11 = this.Element11;
            result.Element12 = this.Element21;
            result.Element13 = this.Element31;

            result.Element20 = this.Element02;
            result.Element21 = this.Element12;
            result.Element22 = this.Element22;
            result.Element23 = this.Element32;

            result.Element30 = this.Element03;
            result.Element31 = this.Element13;
            result.Element32 = this.Element23;
            result.Element33 = this.Element33;

            return result;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Transforms the specified vector with the matrix.
        /// </summary>
        /// <param name="vector">The vector.</param>
        /// <param name="matrix">The matrix.</param>
        /// <returns>Transformed vector.</returns>
        //-----------------------------------------------------------------------------------------

        public static Vector3 Multiply(Vector3 vector, Matrix matrix)
        {
            float x = vector.X * matrix.Element00 + vector.Y * matrix.Element10 + vector.Z * matrix.Element20 + matrix.Element30;
            float y = vector.X * matrix.Element01 + vector.Y * matrix.Element11 + vector.Z * matrix.Element21 + matrix.Element31;
            float z = vector.X * matrix.Element02 + vector.Y * matrix.Element12 + vector.Z * matrix.Element22 + matrix.Element32;
            float w = vector.X * matrix.Element03 + vector.Y * matrix.Element13 + vector.Z * matrix.Element23 + matrix.Element33;

            if(w == 1.0f)
            {
                return new Vector3(x, y, z);
            }

            else if(w != 0.0f)
            {
                return new Vector3(x, y, z) / w;
            }

            throw new DivideByZeroException();
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Implements the vector/matrix multiplication operator.
        /// </summary>
        /// <param name="vector">The vector.</param>
        /// <param name="matrix">The matrix.</param>
        /// <returns>Transformed vector.</returns>
        //-----------------------------------------------------------------------------------------

        public static Vector3 operator *(Vector3 vector, Matrix matrix)
		{
			return Multiply(vector, matrix);
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Transforms a unit vector with the matrix.
        /// </summary>
        /// <remarks>
        /// Because the vector represents a direction, translation doesn't affect the 
        /// transformation.
        /// </remarks>
        /// <param name="vector">The vector.</param>
        /// <param name="matrix">The matrix.</param>
        /// <returns>Transformed vector.</returns>
        //-----------------------------------------------------------------------------------------

        public static Vector3 MultiplyDirection(Vector3 vector, Matrix matrix)
        {
            float x = vector.X * matrix.Element00 + vector.Y * matrix.Element10 + vector.Z * matrix.Element20;
            float y = vector.X * matrix.Element01 + vector.Y * matrix.Element11 + vector.Z * matrix.Element21;
            float z = vector.X * matrix.Element02 + vector.Y * matrix.Element12 + vector.Z * matrix.Element22;

            return new Vector3(x, y, z);
        }


        public static Vector3 MultiplyNormal(Vector3 vector, Matrix matrix)
        {
            float x = vector.X * matrix.Element00 + vector.Y * matrix.Element01 + vector.Z * matrix.Element02;
            float y = vector.X * matrix.Element10 + vector.Y * matrix.Element11 + vector.Z * matrix.Element12;
            float z = vector.X * matrix.Element20 + vector.Y * matrix.Element21 + vector.Z * matrix.Element22;

            return new Vector3(x, y, z);
        }


        public static BoundingBox Multiply(BoundingBox boundingBox, Matrix matrix)
        {
            // Start with the translation portion
            float minX = matrix.TranslationPart.X;
            float minY = matrix.TranslationPart.Y;
            float minZ = matrix.TranslationPart.Z;

            float maxX = matrix.TranslationPart.X;
            float maxY = matrix.TranslationPart.Y;
            float maxZ = matrix.TranslationPart.Z;

            // Examine each of the 9 matrix elements
            // and compute the new AABB

            if (matrix.Element00 > 0.0f)
            {
                minX += matrix.Element00 * boundingBox.MinPoint.X; maxX += matrix.Element00 * boundingBox.MaxPoint.X;
            }
            else
            {
                minX += matrix.Element00 * boundingBox.MaxPoint.X; maxX += matrix.Element00 * boundingBox.MinPoint.X;
            }

            if (matrix.Element01 > 0.0f)
            {
                minY += matrix.Element01 * boundingBox.MinPoint.X; maxY += matrix.Element01 * boundingBox.MaxPoint.X;
            }
            else
            {
                minY += matrix.Element01 * boundingBox.MaxPoint.X; maxY += matrix.Element01 * boundingBox.MinPoint.X;
            }

            if (matrix.Element02 > 0.0f)
            {
                minZ += matrix.Element02 * boundingBox.MinPoint.X; maxZ += matrix.Element02 * boundingBox.MaxPoint.X;
            }
            else
            {
                minZ += matrix.Element02 * boundingBox.MaxPoint.X; maxZ += matrix.Element02 * boundingBox.MinPoint.X;
            }

            if (matrix.Element10 > 0.0f)
            {
                minX += matrix.Element10 * boundingBox.MinPoint.Y; maxX += matrix.Element10 * boundingBox.MaxPoint.Y;
            }
            else
            {
                minX += matrix.Element10 * boundingBox.MaxPoint.Y; maxX += matrix.Element10 * boundingBox.MinPoint.Y;
            }

            if (matrix.Element11 > 0.0f)
            {
                minY += matrix.Element11 * boundingBox.MinPoint.Y; maxY += matrix.Element11 * boundingBox.MaxPoint.Y;
            }
            else
            {
                minY += matrix.Element11 * boundingBox.MaxPoint.Y; maxY += matrix.Element11 * boundingBox.MinPoint.Y;
            }

            if (matrix.Element12 > 0.0f)
            {
                minZ += matrix.Element12 * boundingBox.MinPoint.Y; maxZ += matrix.Element12 * boundingBox.MaxPoint.Y;
            }
            else
            {
                minZ += matrix.Element12 * boundingBox.MaxPoint.Y; maxZ += matrix.Element12 * boundingBox.MinPoint.Y;
            }

            if (matrix.Element20 > 0.0f)
            {
                minX += matrix.Element20 * boundingBox.MinPoint.Z; maxX += matrix.Element20 * boundingBox.MaxPoint.Z;
            }
            else
            {
                minX += matrix.Element20 * boundingBox.MaxPoint.Z; maxX += matrix.Element20 * boundingBox.MinPoint.Z;
            }

            if (matrix.Element21 > 0.0f)
            {
                minY += matrix.Element21 * boundingBox.MinPoint.Z; maxY += matrix.Element21 * boundingBox.MaxPoint.Z;
            }
            else
            {
                minY += matrix.Element21 * boundingBox.MaxPoint.Z; maxY += matrix.Element21 * boundingBox.MinPoint.Z;
            }

            if (matrix.Element22 > 0.0f)
            {
                minZ += matrix.Element22 * boundingBox.MinPoint.Z; maxZ += matrix.Element22 * boundingBox.MaxPoint.Z;
            }
            else
            {
                minZ += matrix.Element22 * boundingBox.MaxPoint.Z; maxZ += matrix.Element22 * boundingBox.MinPoint.Z;
            }

            return new BoundingBox(new Vector3(minX, minY, minZ), new Vector3(maxX, maxY, maxZ));
        }


        public static BoundingBox operator *(BoundingBox boundingBox, Matrix matrix)
        {
            return Multiply(boundingBox, matrix);
        }

        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Transforms the specified ray with the matrix.
        /// </summary>
        /// <param name="ray">The ray.</param>
        /// <param name="matrix">The matrix.</param>
        /// <returns>Transformed ray.</returns>
        //-----------------------------------------------------------------------------------------

        public static Ray Multiply(Ray ray, Matrix matrix)
        {
            return new Ray(ray.Origin * matrix, Matrix.MultiplyDirection(ray.Direction, matrix), ray.MinimumT, ray.MaximumT);
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Implements the ray/matrix multiplication operator.
        /// </summary>
        /// <param name="ray">The ray.</param>
        /// <param name="matrix">The matrix.</param>
        /// <returns>Transformed ray.</returns>
        //-----------------------------------------------------------------------------------------

        public static Ray operator *(Ray ray, Matrix matrix)
        {
            return Multiply(ray, matrix);
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Multiplies the specified matrix.
        /// </summary>
        /// <param name="matrix1">The first matrix.</param>
        /// <param name="matrix2">The second matrix.</param>
        /// <returns>Concatenation of the two matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix Multiply(Matrix matrix1, Matrix matrix2)
		{
			Matrix result = new Matrix();

			//Compute the first row
            result.Element00 = matrix1.Element00 * matrix2.Element00 + matrix1.Element01 * matrix2.Element10 + matrix1.Element02 * matrix2.Element20 + matrix1.Element03 * matrix2.Element30;
            result.Element01 = matrix1.Element00 * matrix2.Element01 + matrix1.Element01 * matrix2.Element11 + matrix1.Element02 * matrix2.Element21 + matrix1.Element03 * matrix2.Element31;
            result.Element02 = matrix1.Element00 * matrix2.Element02 + matrix1.Element01 * matrix2.Element12 + matrix1.Element02 * matrix2.Element22 + matrix1.Element03 * matrix2.Element32;
            result.Element03 = matrix1.Element00 * matrix2.Element03 + matrix1.Element01 * matrix2.Element13 + matrix1.Element02 * matrix2.Element23 + matrix1.Element03 * matrix2.Element33;

			//Compute the second row
            result.Element10 = matrix1.Element10 * matrix2.Element00 + matrix1.Element11 * matrix2.Element10 + matrix1.Element12 * matrix2.Element20 + matrix1.Element13 * matrix2.Element30;
            result.Element11 = matrix1.Element10 * matrix2.Element01 + matrix1.Element11 * matrix2.Element11 + matrix1.Element12 * matrix2.Element21 + matrix1.Element13 * matrix2.Element31;
            result.Element12 = matrix1.Element10 * matrix2.Element02 + matrix1.Element11 * matrix2.Element12 + matrix1.Element12 * matrix2.Element22 + matrix1.Element13 * matrix2.Element32;
            result.Element13 = matrix1.Element10 * matrix2.Element03 + matrix1.Element11 * matrix2.Element13 + matrix1.Element12 * matrix2.Element23 + matrix1.Element13 * matrix2.Element33;

			//Compute the third row
            result.Element20 = matrix1.Element20 * matrix2.Element00 + matrix1.Element21 * matrix2.Element10 + matrix1.Element22 * matrix2.Element20 + matrix1.Element23 * matrix2.Element30;
            result.Element21 = matrix1.Element20 * matrix2.Element01 + matrix1.Element21 * matrix2.Element11 + matrix1.Element22 * matrix2.Element21 + matrix1.Element23 * matrix2.Element31;
            result.Element22 = matrix1.Element20 * matrix2.Element02 + matrix1.Element21 * matrix2.Element12 + matrix1.Element22 * matrix2.Element22 + matrix1.Element23 * matrix2.Element32;
            result.Element23 = matrix1.Element20 * matrix2.Element03 + matrix1.Element21 * matrix2.Element13 + matrix1.Element22 * matrix2.Element23 + matrix1.Element23 * matrix2.Element33;

			//Compute the fourth row
            result.Element30 = matrix1.Element30 * matrix2.Element00 + matrix1.Element31 * matrix2.Element10 + matrix1.Element32 * matrix2.Element20 + matrix1.Element33 * matrix2.Element30;
            result.Element31 = matrix1.Element30 * matrix2.Element01 + matrix1.Element31 * matrix2.Element11 + matrix1.Element32 * matrix2.Element21 + matrix1.Element33 * matrix2.Element31;
            result.Element32 = matrix1.Element30 * matrix2.Element02 + matrix1.Element31 * matrix2.Element12 + matrix1.Element32 * matrix2.Element22 + matrix1.Element33 * matrix2.Element32;
            result.Element33 = matrix1.Element30 * matrix2.Element03 + matrix1.Element31 * matrix2.Element13 + matrix1.Element32 * matrix2.Element23 + matrix1.Element33 * matrix2.Element33;

			//Return the result
			return result;
		}


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Implements the matrix multiplication operator.
        /// </summary>
        /// <param name="matrix1">The first matrix.</param>
        /// <param name="matrix2">The second matrix.</param>
        /// <returns>Concatenation of the two matrix.</returns>
        //-----------------------------------------------------------------------------------------

        public static Matrix operator *(Matrix matrix1, Matrix matrix2)
        {
            return Multiply(matrix1, matrix2);
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Determines whether the specified <see cref="T:System.Object"/> is equal to the 
        /// current <see cref="T:System.Object"/>.
        /// </summary>
        /// <param name="obj">
        /// The <see cref="T:System.Object"/> to compare with the current 
        /// <see cref="T:System.Object"/>.
        /// </param>
        /// <returns>
        /// true if the specified <see cref="T:System.Object"/> is equal to the current 
        /// <see cref="T:System.Object"/>; otherwise, false.
        /// </returns>
        /// <exception cref="T:System.NullReferenceException">
        /// The <paramref name="obj"/> parameter is null.
        /// </exception>
        //-----------------------------------------------------------------------------------------

        public override bool Equals(object obj)
        {
            return Equals(obj as Matrix);
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Indicates whether the current object is equal to another object of the same type.
        /// </summary>
        /// <param name="other">An object to compare with this object.</param>
        /// <returns>
        /// true if the current object is equal to the <paramref name="other"/> parameter; 
        /// otherwise, false.
        /// </returns>
        //-----------------------------------------------------------------------------------------
        
        public bool Equals(Matrix other)
        {
            if(other == null)
            {
                throw new ArgumentNullException("other");
            }

            return this.Element00 == other.Element00 && this.Element01 == other.Element01 && this.Element02 == other.Element02 && this.Element03 == other.Element03 &&
                   this.Element10 == other.Element10 && this.Element11 == other.Element11 && this.Element12 == other.Element12 && this.Element13 == other.Element13 &&
                   this.Element20 == other.Element20 && this.Element21 == other.Element21 && this.Element22 == other.Element22 && this.Element23 == other.Element23 &&
                   this.Element30 == other.Element30 && this.Element31 == other.Element31 && this.Element32 == other.Element32 && this.Element33 == other.Element33;
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Serves as a hash function for a particular type.
        /// </summary>
        /// <returns>
        /// A hash code for the current <see cref="T:System.Object"/>.
        /// </returns>
        //-----------------------------------------------------------------------------------------

        public override int GetHashCode()
        {
            return base.GetHashCode();
        }


        //-----------------------------------------------------------------------------------------
        /// <summary>
        /// Returns a <see cref="T:System.String"/> that represents the current 
        /// <see cref="Matrix"/> object.
        /// </summary>
        /// <returns>
        /// A <see cref="T:System.String"/> that represents the current <see cref="Matrix"/>.
        /// </returns>
        //-----------------------------------------------------------------------------------------

        public override string ToString()
        {
            return string.Format(CultureInfo.InvariantCulture, "[{0} {1} {2} {3}]\n[{4} {5} {6} {7}]\n[{8} {9} {10} {11}]\n[{12} {13} {14} {15}]", this.Element00, this.Element01, this.Element02, this.Element03, this.Element10, this.Element11, this.Element12, this.Element13, this.Element20, this.Element21, this.Element22, this.Element23, this.Element30, this.Element31, this.Element32, this.Element33);
        }
    }
}
